﻿using System;
using System.Threading;

namespace Coding4Fun.BoxingBots.Robot
{
	public class BoxerBot : RobotChassis, IDisposable
	{
		readonly PhidgetController _controller;

		public SensorData SensorData;
	
		public bool PollSensors = true;

		const int LeftArmRelayIndex = 2;
		const int RightArmRelayIndex = 0;
		const int HeadRelayIndex = 1;

		const int TopLeftIndex = 0;
		const int TopRightIndex = 3;
		const int BottomLeftIndex = 1;
		const int BottomRightIndex = 2;

		const int LeftSwitchIndex = 0;
		const int RightSwitchIndex = 1;

		public BoxerBot() : this(5, 10, false) { }

		public BoxerBot(int leftSerialPort, int rightSerialPort, bool isMotorEnabled) : base(leftSerialPort, rightSerialPort, isMotorEnabled)
        {
			_controller = new PhidgetController();

			ThreadPool.QueueUserWorkItem(state => 
			{
				while (PollSensors)
				{
					if (_controller.Sensors != null && _controller.Sensors.IsConnected)
					{
						SensorData.RawTopLeft = _controller.Sensors.PollAnalogInput(TopLeftIndex);
						SensorData.RawTopRight = _controller.Sensors.PollAnalogInput(TopRightIndex);
						SensorData.RawBottomLeft = _controller.Sensors.PollAnalogInput(BottomLeftIndex);
						SensorData.RawBottomRight = _controller.Sensors.PollAnalogInput(BottomRightIndex);

						SensorData.IsLeftSwitchTriggered = _controller.Sensors.PollDigitalInput(LeftSwitchIndex);
						SensorData.IsRightSwitchTriggered = _controller.Sensors.PollDigitalInput(RightSwitchIndex);
					}

					Thread.Sleep(33);
				}
			});
        }

		public void SetHealth(int value)
		{
			if (_controller.Leds != null)
				_controller.Leds.SetHealth(value);
		}

		public void ExtendRightArm()
		{
			ExtendArm(RightArmRelayIndex);
		}

		public void ExtendLeftArm()
		{
			ExtendArm(LeftArmRelayIndex);
		}

		public void ExtendHead()
		{
			if(_controller.Relays != null)
				_controller.Relays.Open(HeadRelayIndex, 500);

			if (_controller.Leds != null)
				_controller.Leds.DisplayDead();
		}

		public void RetractHead()
		{
			if (_controller.Relays != null)
				_controller.Relays.Close(HeadRelayIndex);

			if (_controller.Leds != null)
				_controller.Leds.DisplayAlive();
		}

		private void ExtendArm(int index)
		{
			if (_controller.Relays != null)
				_controller.Relays.Open(index, 100);
		}

		public new void Dispose()
		{
			Dispose(true);
			base.Dispose();

			GC.SuppressFinalize(this);
		}

		public Speed CalculateSpeed(double throttleVector, double strafeVector, double rotationAngle)
		{
			rotationAngle = VerifyLegalValues(rotationAngle);
			rotationAngle = AdjustValueForDeadzone(rotationAngle, AllowedRotationAngle, _negatedAllowedRotationAngle);
			
			// miss wired, easy fix is here
			throttleVector *= -1;
			rotationAngle *= -1;
			
			// miss wired, had to flip throttle and straff for calc
			return CalculateSpeed(strafeVector + rotationAngle, throttleVector, strafeVector - rotationAngle, throttleVector);
		}

		public void Drive(double throttleVector, double strafeVector, double rotationAngle)
		{
			Drive(CalculateSpeed(throttleVector, strafeVector, rotationAngle));
		}

		private double _allowedRotationAngle;
		private double _negatedAllowedRotationAngle = 1;
		public double AllowedRotationAngle
		{
			get { return _allowedRotationAngle; }
			set
			{
				_allowedRotationAngle = Math.Abs(VerifyLegalValues(value));

				if (!AllowedRotationAngle.Equals(1d))
					_negatedAllowedRotationAngle = MaxThrottle / (1 - AllowedRotationAngle);
				else
					_negatedAllowedRotationAngle = MaxThrottle;
			}
		}

		protected new virtual void Dispose(bool disposing)
		{
			PollSensors = false;

			if (_controller != null)
				_controller.Dispose();

			base.Dispose(disposing);
		}
	}
}
